#ifndef __JASONACTIONMOVE_H__
#define __JASONACTIONMOVE_H__

#include "JasonAction.h"

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "nav_msgs/Odometry.h"
#include "kobuki_msgs/BumperEvent.h"
#include "jason_msgs/event.h"
#include "jason_msgs/action.h"
#include "ft_nodes/collision_check.h"

#include <boost/algorithm/string.hpp>
#include <boost/timer.hpp>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

class CJasonActionMove : public CJasonAction {
    
    protected:
        
        ros::NodeHandle &               m_rNode;
        ros::Publisher                  m_oCmdvelPub;
        ros::Publisher                  m_oEventPub;
        ros::Subscriber                 m_oOdomSub;
        ros::Subscriber                 m_oBumperSub;
        ros::ServiceClient              m_oCAclient;
        


        geometry_msgs::Twist            m_oMsg;
        geometry_msgs::Point            m_oInitialPosition;
        tf::Quaternion                  m_oInitialPose;
        
        float                           m_fDistance;
        float                           m_fLastDistance;
        unsigned int                    m_wwId;
        std::string                     m_strAction;
        std::string                     m_strAgent;

        bool                            m_bnHasInitialOdometry;
        bool                            m_bnBlocked;
        boost::posix_time::ptime        m_oProgressTimeout;
        
    private:
        
        const float     GetDistance     ( const geometry_msgs::Point & );
    
    public:
        
        CJasonActionMove( ros::NodeHandle &, const jason_msgs::action::ConstPtr& );
        virtual ~CJasonActionMove();
        
        virtual void    Step            ( void );
        virtual void    Start           ( void );
        
        void            ReceiveOdometry ( const nav_msgs::Odometry::ConstPtr& );
        void            BumperStatus    ( const kobuki_msgs::BumperEvent::ConstPtr& );        
};


#endif //__JASONACTIONMOVE_H__
